#include "enc.h"

#include "oled.h"
#include "tim.h"

uint8_t Enc_Init(Enc_HandleTypeDef *handle) {
  // 初始化输出
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);

  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);

  __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, 0);

  // 初始化输入
  // 初始化编码器输入
  HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
  handle->count1 = 0;
  handle->count2 = 0;
  handle->pos1 = 0;
  handle->pos2 = 0;
  __HAL_TIM_SET_COUNTER(&htim1, 0);
  __HAL_TIM_SET_COUNTER(&htim2, 0);

  return 0;
}

void Enc_Update(Enc_HandleTypeDef *handle) {
  handle->count1 = __HAL_TIM_GetCounter(&htim1) * 0.8 + handle->count1 * 0.2;
  handle->count2 = __HAL_TIM_GetCounter(&htim2) * 0.8 + handle->count2 * 0.2;
  __HAL_TIM_SET_COUNTER(&htim1, 0);
  __HAL_TIM_SET_COUNTER(&htim2, 0);
  handle->pos1 += handle->count1 * 4;
  handle->pos2 += handle->count2;
  handle->val = (handle->count1 + handle->count2) / 2;
}

void Enc_SetSpeed(Enc_HandleTypeDef *handle, float speed1, float speed2,
                  float beta) {
  // 计算占空比
  int pwm1 = speed1 * 1024 * 0.23 - 1024 * beta * 0.05;
  int pwm2 = speed2 * 1024 * 0.85 + 1024 * beta * 0.2;
  Oled_ShowNumSmall(0, 3, pwm1, 10);
  Oled_ShowNumSmall(60, 3, pwm2, 10);
  // 设置占空比
  if (pwm1 >= 0) {
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, pwm1);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
  } else {
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, -pwm1);
  }
  if (pwm2 >= 0) {
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, 0);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, pwm2);
  } else {
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, -pwm2);
  }
}

void Enc_SetPos(Enc_HandleTypeDef *handle, int64_t pos1, int64_t pos2) {
  handle->pos1 = pos1;
  handle->pos2 = pos2;
}
